#include <iostream>
#include <set>
#include <cmath>

#include "TetrahedronIntersection.h"
#include "SweepDescription.h"
#include "Vector.h"

#include "PArray.h"
#include "Tetrahedron.h"
#include "Triangle.h"
#include "Node.h"
#include "FEMModel.h"
#include "Ray.h"
#include "ClosestVertexSelector.h"
#include "Simplex.h"
#include "EdgeIntersection.h"

#if defined(_WIN32)
#include <windows.h>
#endif

#include <gl/GL.h>
#include <gL/GLU.h>

void ClosestVertexSelector::Visualize (FEMDrawer * draw)const
{
	glPointSize (5.0);

	glBegin (GL_POINTS);
	glColor4f (0,0,0,1);
	for (EdgeInterMap::const_iterator i(edge_inters_.begin());
		i != edge_inters_.end(); i++)
	{
		if ((*i).second)
			glVertex3dv ((*i).second->point_.elts_ );
	}
	glEnd ();
}



ClosestVertexSelector::~ClosestVertexSelector ()
{
	JunkCurrentCut ();
}

TetrahedronIntersection * ClosestVertexSelector::GetTetrahedronIntersection (Tetrahedron * tet, TetrahedronIntersection * incomplete)
{
	Array<Simplex> es (tet->Edges ());

	TetrahedronIntersection ti;
	TetrahedronIntersection *tip = incomplete  ? incomplete : &ti;

	for (int i=es.Size (); i--; )
	{
		// could use symmetry.
		EdgeIntersection * ei = 0;

		EdgeInterMap::const_iterator it (edge_inters_.find (es[i]));

		if (it == edge_inters_.end ())
		{
			bool clip;
			ei=GetEdgeInter (es[i], &clip);
			edge_inters_[es[i]] = ei;

			if (clip)
				tip->complete_b_ = false;
		}
		else
			ei = (*it).second;

		if (ei && !tip->edges_.Findl (ei))
		{
			tip->edges_.Push (ei);
		}
	}

	if (tip->edges_.Empty ())
		return 0;

	tip->tetra_ = tet;
	if (!incomplete)
	{
		tip = new TetrahedronIntersection (*tip);
		tip->sweep_count_ = desc_->count_;
	}
	tip->triangle_ = tip->GetTriangle ();
	if (tip->triangle_)
	{
		if (!tip->triangle_->Mate ())
			tip->triangle_ = 0;
		tip->complete_b_ = true;
	}

	return tip;
}

ClosestVertexSelector::ClosestVertexSelector (FiniteElementModel *f) : SurfaceSelector (f)
{
}


int center_edge_inter;

LinkArray<Triangle> ClosestVertexSelector::SelectSurface ()
{
	LinkArray<Triangle> retval;
	LinkArray<Tetrahedron> candidates;
	Array<int> int_candidates;

#if 1
	candidates.Concat (fem_->TetrahedronArray ());
#endif  

	/*
	We used to propagate the intersections from the set tets that were
	incompletely cut during the last step. Unfortunately, this doesn't
	work well if the mesh moves (relaxation) or if the scalpel has to
	bridge a gap.


	*/

	/*
	candidates: intersect the sweep surface with the boundary of the
	model.
	*/
	if (!candidates.Size ())
	{
		Tetrahedron * start_tet=0;

		std::set<Triangle*>::const_iterator bt (fem_->OuterFacesBegin ());
		std::set<Triangle*>::const_iterator et (fem_->OuterFacesEnd ());

		for (; !start_tet && bt!=et;  bt++)
		{
			for (int j=3; j--; )
			{
				Simplex ts =  (*bt) ->GetSet ().GetSubset (j);

				if (ts.Parity ())
					ts = ts.GetMate ();

				bool clip;

				EdgeIntersection * ei=GetEdgeInter (ts, &clip);

				// do something sensible if inter is  on boundary.
				if (ei)
				{
					start_tet = (*bt)->Tetra ();

					break ;
				}
				edge_inters_[ts] = ei;
			}
		}

		if (!start_tet)
			return retval;
		candidates.Push (start_tet);
	}

	LinkArray<TetrahedronIntersection> t_inters;
	while (candidates.Size( ))
	{
		Tetrahedron * tet = candidates.Pop ();

		if (!tet)
			cout << "huh? null candidate";

		TetrahedronInterMap::const_iterator it (tet_inters_.find (tet));
		TetrahedronIntersection * ti =
			(it == tet_inters_.end ())
			? 0 : tet_inters_[tet];

		if (ti && (ti->complete_b_ || ti->sweep_count_ == desc_->count_ ))
			continue;

		ti  = GetTetrahedronIntersection  (tet, ti);
		if (ti)
		{
			candidates.Concat (ti->AdjacentTetras ());
			tet_inters_[tet] = ti;
			ti->sweep_count_ = desc_->count_;
			Triangle* t = ti->triangle_;
			if (t)
			{
				retval.Push (t);
			}

			t_inters.Push (ti);
		}
	}


	{
		/*
		do some stats: how is # degens linked with number of nodes
		selected by "center-edge" intersections. We look at all the edge
		intersections leading to the selection of a node.  */

		map<Node*,bool> nods_center_edge;
		for (int i = retval.Size(); i--;)
			for (int j = 3; j--;)
				nods_center_edge[retval[i]->GetNode(j)] = true;

		for (int i = t_inters.Size (); i--;)
		{
			TetrahedronIntersection * ti = t_inters[i];
			for (int j = ti->edges_.Size ();j--;)
			{
				EdgeIntersection*ei = ti->edges_[j];
				nods_center_edge[ei->closest_] =
					ei->center_b_ && nods_center_edge[ei->closest_];
			}
		}

		for (map<Node*,bool>::const_iterator i = nods_center_edge.begin ();
			i != nods_center_edge.end (); i++)
		{
			if ((*i).second)
				center_edge_inter++;
		}
	}

	return retval;
}

/*
Get the intersection of the current sweep and E.

RETURN

The intersection or NULL if failed.v

CLIPPED is set if they do intersect, but the intersection point falls outside
the current sweep.

*/

/*
Hmm. interesting. We intersect all edges, which means that CLIPPED
is set for a very large no. of edges */
EdgeIntersection *ClosestVertexSelector::GetEdgeInter (Simplex const & e, bool *clipped_b)
{
	Ray r;

	Node *n1 = e.GetNode (0);	// huh, order? 
	Node *n2 = e.GetNode (1);
	r.origin_ = n1->Location (true);
	r.dir_ =  n2->Location (true) - n1->Location(true);

	*clipped_b = false;

	/*
	cut and paste. Sue me.
	*/

	Real dot = desc_->Normal () * r.dir_;
	if (fabs (dot) < 1e-8)	// ugh
		return 0;

	Real dist = (desc_->Off() -  r.origin_ * desc_->Normal ()) / dot;

	if (dist < 0 || dist > 1)
		return 0;

	Vector3 point = r.dir_ * dist + r.origin_;

	if (desc_->Contains (point))
	{
		EdgeIntersection * ei = new EdgeIntersection;
		ei->sweep_count_ = desc_->count_;
		ei->where_ = dist;
		ei->point_ = point;
		ei->edge_  = e;
		Node *np = 0;

		/*
		We have tried to do something intelligent:

		* if the edge (n1,n2)
		is not on the outside, but n1 and n2 both are (we don't select
		either one),

		* or if only one of them is (then we try snapping to the
		surface node automatically.)

		It does not decrease the number of degeneracies, but only
		creates more, it seems, so this code is removed. Now that
		Node::outer_tri_ also doesn't exist, this is again not implementable here.
		*/
		if (np == 0)
			np = (dist > 0.5)? n2 : n1;

		ei->center_b_ = fabs (dist - 0.5) < 0.1666;
		ei->closest_ = np;

		return ei;
	}
	else
	{
		*clipped_b = true;
		return 0;
	}
}

/*
Try repositioning all nodes in NS , and all pending nodes from
previous cuts.  This has to be a function separate from
SelectSurface (), since SelectSurface () can also be used to `try'
surfaces.

TODO:

should bound the length of the position change; if the scalpel
rotates 180 degs, a vertex might be projected long after it was
eligible, causing an inverted tet.

*/

void ClosestVertexSelector::RepositionNodes (Array<TetnodTuple> ns)
{
	for (int i = ns.Size (); i--;)
		to_reposition_node_.insert (ns[i]);

	std::set<TetnodTuple> left ;
	for (std::set<TetnodTuple>::const_iterator i (to_reposition_node_.begin ());
		i != to_reposition_node_.end ();
		i++)
	{
		TetnodTuple tn  (*i);
		Node * n = tn.node_;
		Vector3 undeformed, deformed;

		LinkArray<Tetrahedron> cell = FindCell (tn.tetra_, n);

		bool k = RepositionNode (n, &undeformed, &deformed, cell );
		if (!k)
			left.insert (tn);
		else
		{
			fem_->ScheduleMoveNode (n, undeformed, deformed, cell);
			AddSnappedNode (cell, n);
		}
	}
	to_reposition_node_ = left;
}



/*
a node that is involved in the cutting process.  This
must happen before DISSECT, since repositioning topological
information (namely: whether a vertex lies on the boundary of the
complex.)
*/
bool ClosestVertexSelector::RepositionNode (Node* nod, Vector3 * undeformedp , Vector3 *deformedp,
										  LinkArray<Tetrahedron> cell)
{
	Vector3 pos = nod->Location (true);
	Vector3 deformed;

	/*
	Decide where the projection of NOD onto the sweep is.  The surface
	case is a trifle complicated, since we can only move the node
	parallel to the surface.  */
	LinkArray<Triangle> surface;
	for (int i = cell.Size () ; i-- ; )
		for (int j = 4; j --;)
		{
			Triangle *tr =cell[i]->GetTriangle( j);
			if (!tr->Mate ()
				&& tr->ContainsB (nod))
				surface.Push (cell[i]->GetTriangle (j));
		}

		if (surface.Size ())
		{
			/*
			Try all orthogonal projections of the node within each of the
			surface tris, and use the closest. Go in denial if we find none.
			*/
			Real best_distance = 1e10;

			for (int i=surface.Size(); i--;)
			{
				Vector3 tn = surface[i]->Normal (true);
				Real t_off = surface[i]->NormalInnerProduct (true);

				Vector3 dn =desc_->Normal ();
				Real d_off = desc_->Off ();

				Vector3 m = Vector3::Cross (dn,tn);

				/*
				M is the direction of  the intersection of DESC and OUTER_TRI
				*/
				Vector3 k = Vector3::Cross (dn, m);
				k.Normalize();
				m.Normalize ();

				Real gamma = (t_off - d_off * (dn * tn)) /  (k * tn);
				Vector3 x = dn * d_off + gamma * k;
				/*
				X is on the intersection of DESC and OUTER_TRI 
				*/
				Vector3 this_deformed = x + ((pos - x) * m)  * m;

				if (surface[i]->ContainsB (this_deformed, true))
				{
					Real distance = (this_deformed - pos).Length2();
					if (distance < best_distance)
					{
						best_distance = distance;
						deformed = this_deformed;
					}
				}
			}

			if (best_distance >= 1e8)
			{
				/*
				We're in denial here; second guess: we calculate the
				average surface normal, and project onto that.  */
				Vector3 tn;
				for (int i=surface.Size(); i--;)
					tn += surface[i]->Normal (true);

				tn.Normalize();
				Real t_off = tn * pos;

				Vector3 dn =desc_->Normal ();

				Real d_off = desc_->Off ();

				Vector3 m = Vector3::Cross (dn,tn);

				/*
				M is the direction of  the intersection of DESC and OUTER_TRI
				*/
				Vector3 k = Vector3::Cross (dn, m);
				k.Normalize();
				m.Normalize ();

				Real gamma = (t_off - d_off * (dn * tn)) /  (k * tn);
				Vector3 x = dn * d_off + gamma * k;
				/*
				X is on the intersection of DESC and OUTER_TRI 
				*/

				deformed = x + ((pos - x) * m)  * m;

				/*
				TODO: should calculate variance of vectors, to estimate
				whether the avg is a good idea?  */

			}
		}
		else
		{
			deformed =  pos + (desc_->Off () - desc_->Normal ()*  pos)  * desc_->Normal ();
		}


		/*
		randomly moving around stuff looks weird if it happens
		on the surface. Don't do it.

		Also don't do it if it 's on the inside.


		(what does this comment apply to ?  --hwn  2/2/2001)
		*/


		Tetrahedron *tet= 0;

#if 0
		if (!desc_->contains (deformed))
		{
			return false;
		}
#endif  
		Triangle * outer = 0;


		/*
		Find out which tetra the projected position is in; the tetra
		contains the strain, and is needed to transform back to the
		undeformed case.  */
		if (!tet && surface.Size ())
		{

			for (int i=surface.Size (); !tet && i--;)
				if (surface[i]->ContainsB (deformed, true)
					&& !surface[i]->Tetra ()->DegenerateB ())
				{
					outer = surface[i];
					tet = outer->Tetra();
				}
		}
		else if (!tet)
		{
			for (int i= cell.Size (); !tet && i--;  )
				for (int j= 4; !tet && j--;  )
				{
					if (cell[i]->ContainsB (deformed, true)
						&& !cell[i]->DegenerateB ())
						tet = cell[i];
				}
		}

		if (!tet)
			return false;

		Vector3 undeformed = tet->TransformBack (fem_, deformed);

		bool barf = false;

		barf = barf || (outer && !outer->ContainsB (undeformed, false));
		barf = barf || (!outer && !tet->ContainsB (undeformed, false));

		if (barf)
		{
			cerr << "Huh? New position  of node " << nod->Number () 
				<< " not contained in ";
			if (outer)
				cerr <<  "  triangle " <<outer->Number ();
			else
			{
				cerr << " tetrahedron " << tet->Number () << endl;
				cerr << "def ctcy = " << tet->Containancy (deformed, true)
					<< "undef ctcy = " << tet->Containancy (undeformed, false) <<endl;	  
			}
		}

		*undeformedp  = undeformed;
		*deformedp  = deformed;  
		return true;
}

/*
Make sure that all edge intersections of NOD have NOD as their
closest node.  This is needed to make sure that separate cuts are
connected. Otherwise roundoff errors will cause problems.

*/
void ClosestVertexSelector::AddSnappedNode (LinkArray<Tetrahedron> cell, Node *nod)
{
	LinkArray<Node> cell_nods;
	for (int j = cell.Size (); j-- ; )
		for (int k = 4; k--;)
			cell_nods.Push (cell[j]->GetNode(k));

	cell_nods.DefaultSort ();
	cell_nods.Uniq ();

	for (int j=cell_nods.Size (); j--;)
	{
		if (cell_nods[j] == nod)
			continue;

		Node*n[2] = {cell_nods[j], nod};

		Simplex e (EDGE, n, 0);
		if (e.Parity ())
			e = e.GetMate();


		EdgeIntersection * ei = edge_inters_[e];

		if (!ei)
		{
			ei = new EdgeIntersection;
			ei->edge_ = e;
			edge_inters_[e] = ei;
		}

		ei->moved_node_ = true;
		ei->sweep_count_ = desc_->count_;

		// todo.
		ei->where_ = (nod == e.GetNode(0)) ? 0.0 : 1.0;
		ei->closest_ = nod;
	}
}

void ClosestVertexSelector::JunkCurrentCut ()
{
	for (EdgeInterMap::const_iterator i (edge_inters_.begin ());
		i != edge_inters_.end (); i++)
		delete (*i).second;

	for (TetrahedronInterMap::const_iterator i (tet_inters_.begin ());
		i != tet_inters_.end (); i++)
		delete (*i).second;

	EdgeInterMap empty_e;
	edge_inters_ = empty_e;

	TetrahedronInterMap empty_tets;
	tet_inters_= empty_tets;
}

void ClosestVertexSelector::NextSweep ()
{
	/*
	clean out nil entries: Edges might be intersected by the new
	sweep.
	*/
	EdgeInterMap new_edges;
	for (EdgeInterMap::const_iterator i (edge_inters_.begin ());
		i != edge_inters_.end (); i++)
		if ((*i).second )
			new_edges[(*i).first] = (*i).second;

	edge_inters_ = new_edges;
}


void ClosestVertexSelector::ClearThisSweep  ()
{
	TetrahedronInterMap new_tets;

	/*
	Copy the contents of TET_INTERS_ that was not generated during
	this sweep.
	*/
	for (TetrahedronInterMap::const_iterator  i (tet_inters_.begin ());
		i != tet_inters_.end (); i++)
	{
		TetrahedronIntersection *ti= (*i).second;
		if (ti && ti->sweep_count_ < desc_->count_)
		{
			new_tets[(*i).first] = ti;
			for (int j= ti->edges_.Size(); j--;  )
				if (ti->edges_[j]->sweep_count_ >= desc_->count_)
					ti->edges_.Del (j);
		}
		else
			delete ti;
	}
	tet_inters_ = new_tets;

	EdgeInterMap new_edges;
	for (EdgeInterMap::const_iterator i (edge_inters_.begin ());
		i != edge_inters_.end (); i++)
	{
		EdgeIntersection * ei = (*i).second;
		if (ei && ei->sweep_count_ < desc_->count_)
			new_edges[(*i).first] = ei;
		else
			delete ei;
	}

	edge_inters_ = new_edges;
}

/*
Deletion of degenerate tetrahedra causes lots of objects to become
`invalid'. Here we delete references to invalid objects.

POSTCONDITION:

all topological objects  and  topological sets are valid.
*/
void ClosestVertexSelector::JunkInvalidInformation ()
{
	std::set<TetnodTuple> left ;

	for (std::set<TetnodTuple>::const_iterator i (to_reposition_node_.begin ());
		i != to_reposition_node_.end ();
		i++)
	{
		TetnodTuple t = *i;
		if (t.node_->Valid () && t.tetra_->Valid ()
			&& t.tetra_->ContainsB (t.node_))
			left.insert (t);
	}

	to_reposition_node_ = left;

	TetrahedronInterMap new_tets;
	for (TetrahedronInterMap::const_iterator i(tet_inters_.begin ());
		i != tet_inters_.end ();
		i++)
	{
		Tetrahedron * tet = (*i).first;
		TetrahedronIntersection * ti = (*i).second;
		if (ti->Valid ())
			new_tets[tet] = ti;
	}
	EdgeInterMap new_es;

	/*
	we don't have to mark new nodes as snapped, since we copy their
	EdgeIntersection fields anyway.
	*/
	for (EdgeInterMap::const_iterator i(edge_inters_.begin ());
		i != edge_inters_.end ();
		i++)
	{
		EdgeIntersection *ei = (*i).second;
		if (ei
			&& ei -> Valid ())
			new_es[(*i).first] = ei;
	}

	tet_inters_ = new_tets;
	edge_inters_ = new_es;

#ifdef PARANOID 
	validate_pointers ();
#endif
}


/*
After dissect, a lot of new nodes are added which are duplicates of
snapped nodes, so we must account for them.

PRECONDITION: all topological object are valid.
*/

// todo: edge_inters_ update 
void ClosestVertexSelector::UpdateNodes (AllCutComponentsMap const*acm)
{
#ifdef PARANOID 
	validate_pointers ();
#endif
	cerr << "Updating pointers ... "<<flush;

	for (TetrahedronInterMap::const_iterator i(tet_inters_.begin ());
		i != tet_inters_.end ();
		i++)
	{
		Tetrahedron * tet = (*i).first;
		TetrahedronIntersection * ti = (*i).second;


		LinkArray<EdgeIntersection> new_eis;
		for (int j = ti->edges_.Size (); j--;)
		{
			EdgeIntersection * ei = ti->edges_[j];

			Node * nods[] = {ei->edge_.GetNode (0),
				ei->edge_.GetNode (1)};

			Node *newclosest = ei->closest_;
			AllCutComponentsMap::const_iterator cm[] = {acm->find (nods[0]),
				acm->find (nods[1])};

			for (int k =0; k <2; k++)
				if (cm[k] != acm->end () && cm[k]->second)
				{
					CutComponentMap::const_iterator ki
						=cm[k]->second->find (tet); // c++, we love thee

					if (nods[k] == ei->closest_)
						newclosest = ki->second;
					nods[k] = ki->second;
				}
				if (nods[0] == nods[1])
					continue;

				Simplex newedge (EDGE, nods, 0);

				/*
				seems paradoxical. The init of newedge is parity == 0, but
				sorting NODS can flip the parity
				*/
				if (newedge.Parity ()) 
					newedge = newedge.GetMate ();

				EdgeIntersection * new_ei = edge_inters_[newedge];

				if (!new_ei)
				{
					new_ei = new EdgeIntersection (*ei);

					new_ei->edge_ =  newedge;
					new_ei->closest_ = newclosest;

					new_ei->where_ =
						(new_ei->closest_ == new_ei->edge_.GetNode (0))
						? 0.0 : 1.0; 
					edge_inters_[newedge] = new_ei;
				}

				new_eis.Push (new_ei);
		}

		ti->edges_ = new_eis;
	}

	cerr << "done " <<endl;
}


void ClosestVertexSelector::ValidatePointers ()const
{
#ifndef NDEBUG
	for (TetrahedronInterMap::const_iterator i(tet_inters_.begin ());
		i != tet_inters_.end ();
		i++)
	{
		assert ((*i).first->Valid ());      
		if (TetrahedronIntersection *t = i->second )
			assert (t->Valid ());
	}
	for (EdgeInterMap::const_iterator i(edge_inters_.begin ());
		i != edge_inters_.end ();
		i++)
	{
		assert ((*i).first.ValidNodes ());
		if (i->second)
			assert (i->second->Valid ());
	}
#endif
}